{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 第八周进阶作业"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 利用棋盘格图案和你身边能找到的相机(笔记本或台式机的摄像头，手机相机等均可)完成相机标定，并给出结果的可信性分析。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import glob"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001\n",
    "criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)\n",
    "#棋盘格模板规格\n",
    "w = 11\n",
    "h = 8\n",
    "\n",
    "# 获取标定板角点的位置\n",
    "objp = np.zeros((h*w, 3), np.float32)\n",
    "objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)  # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y\n",
    "\n",
    "obj_points = []  # 存储3D点\n",
    "img_points = []  # 存储2D点\n",
    "\n",
    "#图片存储路径\n",
    "images = glob.glob(\"D:\\qipange\\*.jpg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "5\n"
     ]
    }
   ],
   "source": [
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    size = gray.shape[::-1]\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (w, h), None)\n",
    "    #print(corners)\n",
    "\n",
    "    if ret:\n",
    "        obj_points.append(objp)\n",
    "        corners2 = cv2.cornerSubPix(gray, corners, (5, 4), (-1, -1), criteria)  # 在原角点的基础上寻找亚像素角点\n",
    "        #print(corners2)\n",
    "        if [corners2]:\n",
    "            img_points.append(corners2)\n",
    "        else:\n",
    "            img_points.append(corners)\n",
    "\n",
    "        #cv2.drawChessboardCorners(img, (w, h), corners, ret)  \n",
    "        #x, y = img.shape[0:2]\n",
    "        #tempimg = cv2.resize(img,(int(y / 30), int(x / 30)))\n",
    "        #cv2.imshow('findCorners',img)\n",
    "        #cv2.waitKey()\n",
    "\n",
    "print(len(img_points))\n",
    "#cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ret: 8.394111587985313\n",
      "mtx:\n",
      " [[3.58393215e+03 0.00000000e+00 2.18018064e+03]\n",
      " [0.00000000e+00 3.74637981e+03 1.99420543e+03]\n",
      " [0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n",
      "dist:\n",
      " [[-0.1226243   0.12999407 -0.00938165 -0.00179274 -0.11025782]]\n",
      "rvecs:\n",
      " [array([[-0.12347065],\n",
      "       [-0.02709817],\n",
      "       [ 1.58129032]]), array([[-0.53571431],\n",
      "       [-0.12132633],\n",
      "       [ 1.8403984 ]]), array([[-0.6147893 ],\n",
      "       [-0.33216904],\n",
      "       [-1.24844356]]), array([[-0.60172582],\n",
      "       [-0.24054094],\n",
      "       [-0.48363246]]), array([[-0.7203763 ],\n",
      "       [-0.3457929 ],\n",
      "       [-1.77613152]])]\n",
      "tvecs:\n",
      " [array([[ 2.85179345],\n",
      "       [-3.48875528],\n",
      "       [12.35307758]]), array([[ 4.37260404],\n",
      "       [-2.1636708 ],\n",
      "       [14.75319967]]), array([[-4.88119151],\n",
      "       [ 0.69871308],\n",
      "       [ 9.97865033]]), array([[-4.12948781],\n",
      "       [-4.01024105],\n",
      "       [12.28902975]]), array([[-3.55820266],\n",
      "       [ 2.52184739],\n",
      "       [10.05160225]])]\n"
     ]
    }
   ],
   "source": [
    "# 标定\n",
    "ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)\n",
    "\n",
    "np.savez('C.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)\n",
    "print(\"ret:\", ret)\n",
    "print(\"mtx:\\n\", mtx) # 内参数矩阵\n",
    "print(\"dist:\\n\", dist)  # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)\n",
    "print(\"rvecs:\\n\", rvecs)  # 旋转向量  # 外参数\n",
    "print(\"tvecs:\\n\", tvecs ) # 平移向量  # 外参数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "手机型号为 荣耀magic2，后置相机焦距为 f=3.95mm 分辨率为 4680*3456 ，传感器尺寸为 5.16*3.87,此时相机的dx=0.001103 dy=0.001120,fx=3581.14,fy=3526.79,u0=2340,v0=1728\n",
    "相机标定的结果显示，fx=3.58393215e+03,fy=3.74637981e+03,u0=2.18018064e+03,v0=1.99420543e+03\n",
    "对比结果来看，横向数据较为接近，但纵向数据差距较大，考虑到相机参数只考虑了一个摄像头，实际手机是三摄像头进行拍摄算法，该误差也在接受范围内"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 在1基础上，使用同一相机，将棋盘格放在前方1m左右固定，然后使用线性方法进行相对位姿估计，然后评价结果的合理性。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 加载相机标定的数据\n",
    "with np.load('C.npz') as X:\n",
    "    mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw(img, corners, imgpts):\n",
    "    \"\"\"\n",
    "    在图片上画出三维坐标轴\n",
    "    :param img: 图片原数据\n",
    "    :param corners: 图像平面点坐标点\n",
    "    :param imgpts: 三维点投影到二维图像平面上的坐标\n",
    "    :return:\n",
    "    \"\"\"\n",
    "    corner = tuple(corners[0].ravel())\n",
    "    cv2.line(img, corner, tuple(imgpts[0].ravel()), (255, 0, 0), 5)\n",
    "    cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5)\n",
    "    cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5)\n",
    "    return img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "旋转变量 [[-0.12393196]\n",
      " [-0.01641404]\n",
      " [ 1.5818751 ]]\n",
      "平移变量 [[ 2.87599142]\n",
      " [-3.49129772]\n",
      " [12.40773171]]\n",
      "旋转变量 [[-0.53536669]\n",
      " [-0.12319598]\n",
      " [ 1.840535  ]]\n",
      "平移变量 [[ 4.37997969]\n",
      " [-2.16782285]\n",
      " [14.7295899 ]]\n",
      "旋转变量 [[-0.61837249]\n",
      " [-0.33573756]\n",
      " [-1.25131169]]\n",
      "平移变量 [[-4.88117632]\n",
      " [ 0.71138987]\n",
      " [ 9.93297287]]\n",
      "旋转变量 [[-0.60060997]\n",
      " [-0.24181378]\n",
      " [-0.48520687]]\n",
      "平移变量 [[-4.13981964]\n",
      " [-4.00729398]\n",
      " [12.26001505]]\n",
      "旋转变量 [[-0.69997337]\n",
      " [-0.34648699]\n",
      " [-1.77679791]]\n",
      "平移变量 [[-3.56741823]\n",
      " [ 2.5359207 ]\n",
      " [10.18129345]]\n"
     ]
    }
   ],
   "source": [
    "# 初始化目标坐标系的3D点\n",
    "objp = np.zeros((h * w, 3), np.float32)\n",
    "objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)\n",
    "# 初始化三维坐标系\n",
    "axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)  # 坐标轴\n",
    "# 加载打包所有图片数据\n",
    "\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    # 找到图像平面点坐标点\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (w, h), None)\n",
    "    if ret:\n",
    "        # PnP计算得出旋转向量和平移向量\n",
    "        _, rvecs, tvecs, _ = cv2.solvePnPRansac(objp, corners, mtx, dist)\n",
    "        print(\"旋转变量\", rvecs)\n",
    "        print(\"平移变量\", tvecs)\n",
    "        # 计算三维点投影到二维图像平面上的坐标\n",
    "        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)\n",
    "        # 把坐标显示图片上\n",
    "        img = draw(img, corners, imgpts)\n",
    "        x, y = img.shape[0:2]\n",
    "        tempimg = cv2.resize(img,(int(y / 10), int(x / 10)))\n",
    "        cv2.imshow('img', tempimg)\n",
    "        cv2.waitKey()\n",
    "\n",
    "#cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "该方法直接使用1的图片，所得出的的旋转及平移变量与1的出的较为接近，但考虑到并未对图像进行去除畸变及投影误差的操作，该结果并不准确，还可以进一步优化"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
